#include "ceres_solver_optimize.h"

static Eigen::Matrix<double, 10, 1> ComputeCeresResidual(const Eigen::Matrix<double, 13, 1> &state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity) {
	Eigen::Vector2d res_p = state.head<2>() - last_pose.block<2,1>(0,0);
	// double res_theta = math_utils::normalize_angle(state(4,0) - last_pose[2]);
	double res_theta = math_utils::theta_normalize(state(4,0) - last_pose[2]);

	Eigen::Vector2d res_v = state.block<2,1>(2,0) - last_velocity;

	Eigen::Matrix<double, 10, 1> residual;
	residual.block<2,1>(0,0) = res_p;
	residual.block<2,1>(2,0) = res_v;
	residual(4,0) = res_theta;	

	residual.block<2,1>(5,0) = state.block<2,1>(8,0) - pose.block<2,1>(0,0);
	residual.block<2,1>(7,0) = state.block<2,1>(10,0) - velocity.block<2,1>(0,0);
	residual(9,0) = math_utils::theta_normalize(state(12,0) - pose[2]);	

	return residual;
}

void CeresOptimizer::ComputeResidualAndNumericalJacobian(const State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residualXi, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 1> &residualXj, Eigen::Matrix<double, 5, 5> &jacobianXj, Eigen::Matrix<double, 5, 5> &jacobianXij, Eigen::Matrix<double, 5, 8> &jacobianXji) {
	Eigen::Matrix<double, 13, 1> state_tmp;
	state_tmp.block<2,1>(0,0) = last_state.p;
	state_tmp.block<2,1>(2,0) = last_state.v;
	state_tmp(4,0) = last_state.theta;

	state_tmp.block<2,1>(5,0) = last_state.ba;
	state_tmp(7,0) = last_state.bg;
	state_tmp.block<2,1>(8,0) = state.p;
	state_tmp.block<2,1>(10,0) = state.v;
	state_tmp(12,0) = state.theta;

    auto* cost_function =
            new ceres::NumericDiffCostFunction<  
                PoseNumericalDiff,
                ceres::CENTRAL,  
                10, 13           
            >(new PoseNumericalDiff(last_pose, last_velocity.head<2>(), pose, velocity.head<2>()));

    double x[13];
    for(int i = 0; i < 13; i++) {
        x[i] = state_tmp[i];
    }    

    double residuals[10];  // 10维残差
    double* jacobians[1];  // 雅可比矩阵指针数组
    double jacobian_values[10 * 13];  // 10x13雅可比矩阵(行优先)
    jacobians[0] = jacobian_values;
    const double* parameters[1] = {x};

    cost_function->Evaluate(parameters, residuals, jacobians);

    std::cout << "残差向量(10维):\n";
    for (int i = 0; i < 10; ++i) {
        std::cout << residuals[i] << " ";
    }
    std::cout << "\n\n";

    std::cout << "雅可比矩阵(10x13):\n";
    for (int i = 0; i < 10; ++i) {
        for (int j = 0; j < 13; ++j) {
            std::cout << jacobian_values[i * 13 + j] << "\t";
        }
        std::cout << "\n";
    }
}

void CeresOptimizer::ComputeResidualAndAutoJacobian(const State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residualXi, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 1> &residualXj, Eigen::Matrix<double, 5, 5> &jacobianXj, Eigen::Matrix<double, 5, 5> &jacobianXij, Eigen::Matrix<double, 5, 8> &jacobianXji) {
    // Eigen::Matrix<double, 13, 1> state_tmp;
    // state_tmp.block<2,1>(0,0) = last_state.p;
    // state_tmp.block<2,1>(2,0) = last_state.v;
    // state_tmp(4,0) = last_state.theta;

    // state_tmp.block<2,1>(5,0) = last_state.ba;
    // state_tmp(7,0) = last_state.bg;
    // state_tmp.block<2,1>(8,0) = state.p;
    // state_tmp.block<2,1>(10,0) = state.v;
    // state_tmp(12,0) = state.theta;

    // auto* cost_function =
    //         new ceres::AutoDiffCostFunction<  
    //             PoseAutoDiff,
    //             ceres::CENTRAL,  
    //             10, 13           
    //         >(new PoseAutoDiff(last_pose, last_velocity.head<2>(), pose, velocity.head<2>()));

    // double x[13];
    // for(int i = 0; i < 13; i++) {
    //     x[i] = state_tmp[i];
    // }    

    // double residuals[10];  // 10维残差
    // // double* jacobians[1];  // 雅可比矩阵指针数组
    // // double jacobian_values[10 * 13];  // 10x13雅可比矩阵(行优先)
    // // jacobians[0] = jacobian_values;
    // const double* parameters[1] = {x};

    // // cost_function->Evaluate(parameters, residuals, jacobians);

    // double* residual_ptrs[] = {residuals};
    
    // // 雅可比矩阵存储 (10x13)
    // double jacobian_matrix[10 * 13];
    // double* jacobian_ptrs[] = {jacobian_matrix};
    
    // // 评估代价函数以获取雅可比
    // cost_function->Evaluate(&x, residual_ptrs, jacobian_ptrs);

    // std::cout << "残差向量(10维):\n";
    // for (int i = 0; i < 10; ++i) {
    //     std::cout << residuals[i] << " ";
    // }
    // std::cout << "\n\n";

    // std::cout << "雅可比矩阵(10x13):\n";
    // for (int i = 0; i < 10; ++i) {
    //     for (int j = 0; j < 13; ++j) {
    //         std::cout << jacobian_values[i * 13 + j] << "\t";
    //     }
    //     std::cout << "\n";
    // }
}

void CeresOptimizer::OptimizeGN(State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity) {
	Eigen::Matrix<double, 13, 1> state_tmp;
	state_tmp.block<2,1>(0,0) = last_state.p;
	state_tmp.block<2,1>(2,0) = last_state.v;
	state_tmp(4,0) = last_state.theta;

	state_tmp.block<2,1>(5,0) = last_state.ba;
	state_tmp(7,0) = last_state.bg;
	state_tmp.block<2,1>(8,0) = state.p;
	state_tmp.block<2,1>(10,0) = state.v;
	state_tmp(12,0) = state.theta;

    ceres::Problem problem;

    auto* cost_function =
            new ceres::NumericDiffCostFunction<  
                PoseNumericalDiff,
                ceres::CENTRAL,  
                10, 13           
            >(new PoseNumericalDiff(last_pose, last_velocity.head<2>(), pose, velocity.head<2>()));

    double x[13];
    for(int i = 0; i < 13; i++) {
        x[i] = state_tmp[i];
    }    
#if 0
    double residuals[10];  // 10维残差
    double* jacobians[1];  // 雅可比矩阵指针数组
    double jacobian_values[10 * 13];  // 10x13雅可比矩阵(行优先)
    jacobians[0] = jacobian_values;
    const double* parameters[1] = {x};

    cost_function->Evaluate(parameters, residuals, jacobians);

    std::cout << "残差向量(10维):\n";
    for (int i = 0; i < 10; ++i) {
        std::cout << residuals[i] << " ";
    }
    std::cout << "\n\n";

    std::cout << "雅可比矩阵(10x13):\n";
    for (int i = 0; i < 10; ++i) {
        for (int j = 0; j < 13; ++j) {
            std::cout << jacobian_values[i * 13 + j] << "\t";
        }
        std::cout << "\n";
    }
#endif

    problem.AddResidualBlock(cost_function, nullptr, x);
    
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    std::cout << summary.BriefReport() << "\n";

    Eigen::Matrix<double, 13, 1> state_opt;

    for(int i = 0; i < 13; i++) {
        state_opt[i] = x[i];
    }   

    std::cout << "ceres pose optimize: " << state_opt.transpose() << "\n";

    last_state.p = state_opt.block<2,1>(0,0);
    last_state.v = state_opt.block<2,1>(2,0);
    last_state.theta = state_opt[4];
    // last_state.theta = math_utils::normalize_angle(last_state.theta);
    last_state.theta = math_utils::theta_normalize(last_state.theta);
    last_state.ba = state_opt.block<2,1>(5,0); 
    last_state.bg = state_opt(7,0);

    state.p = state_opt.block<2,1>(8,0);
    state.v = state_opt.block<2,1>(10,0);
    state.theta = state_opt[12];
    // state.theta = math_utils::normalize_angle(state.theta);
    state.theta = math_utils::theta_normalize(state.theta);
    state.ba = last_state.ba; 
    state.bg = last_state.bg;
}

void CeresOptimizer::ExponentialFitting() {
	double m_true = 0.3;
    double c_true = 0.1;
    std::vector<double> x_data, y_data;
    for (double x = 0; x <= 10; x += 0.1) {
        double y = exp(m_true * x + c_true) + 0.1 * rand() / RAND_MAX;
        x_data.push_back(x);
        y_data.push_back(y);
    }

    // 初始参数值
    double m = 0.0;
    double c = 0.0;

    // 构建优化问题
    ceres::Problem problem;
    for (int i = 0; i < x_data.size(); ++i) {
        ceres::CostFunction* cost_function =
            new ceres::AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
                new ExponentialResidual(x_data[i], y_data[i]));
        problem.AddResidualBlock(cost_function, nullptr, &m, &c);
    }

    // 配置求解器
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    // 输出结果
    std::cout << summary.BriefReport() << "\n";
    std::cout << "初始 m: 0.0, c: 0.0\n";
    std::cout << "优化后 m: " << m << ", c: " << c << "\n";
    std::cout << "真实值 m: " << m_true << ", c: " << c_true << "\n";
}
